#include "Aria.h"
#include <iostream>
#include <string.h>

#include "KinectDev.h"
#include "Robot.h"
#include "DogSound.h"

using namespace std;

int main(int argc, char **argv)
{	
	// Initialize kinect ...
	KinectDev kinectDev = KinectDev();
	kinectDev.Init();	

	// Initialize robot ...
	Robot robot = Robot(argc, argv);
	robot.Init();
		
	// Initialize Dog sound ...
	DogSound dog = DogSound();
	dog.Init();	

	// Your code goes here ...

	Coordinate tempCoord;
	float x=0, z=0;
	float velocity = 0;
	// Main loop ...
	dog.Pause();
	while(true)
	{
		ArUtil::sleep(10);

		if( robot.IsStopNecessary() )
		{
			cout<<"Close object"<<endl;
			robot.SetVel(0);
			robot.SetRotVel(0);
			continue;
		}		
		
		if( tempCoord.timeStamp < kinectDev.lastCoordinate.timeStamp ) // new data ...
		{
			dog.Continue();
			tempCoord = kinectDev.lastCoordinate;
			cout<<"New data: "<<z<<endl;

			x = tempCoord.X;
			z = tempCoord.Z;
		
			if ( z < 1.6 )
				velocity = 0;
			else if ( z < 2 )
				velocity = MAX_VELOCITY * ( z - 1.6 );
			else
				velocity = MAX_VELOCITY;

			robot.SetVel( velocity );					
			robot.SetRotVel( (x/z) * 100 );
			dog.Pause();
		}
		else if( GetTime() - tempCoord.timeStamp > 100 )
		{
			ULONGLONG diffTime = GetTime() - tempCoord.timeStamp;
			
			float scale = 1;
			
			if( diffTime < 150 )		// 2000 Initially
				scale = diffTime / (-25) + 1;   // 1000 Initially
			else
				scale = 0;

			velocity = robot.GetCurrentVel() * scale;			
			cout<<velocity<<endl;
			robot.SetVel( velocity );			
			robot.SetRotVel( 0 );
		}

		if( kinectDev.gestureRecognizer.IsNew() )
		{
			dog.Pause();
			PlaySound(TEXT("funny.wav"), NULL, SND_FILENAME | SND_ASYNC);
			Sleep(30);
			if(kinectDev.gestureRecognizer.gestureType == GestureType::RIGHT_HAND_SWIPE_LEFT)
				robot.TurnSync(Directions::LEFT);
			else if(kinectDev.gestureRecognizer.gestureType == GestureType::LEFT_HAND_SWIPE_RIGHT)
				robot.TurnSync(Directions::RIGHT);
			dog.Continue();
		}
	}	

	// Uninitialize Kinect ..
	kinectDev.UnInit();

	// Unitialize Robot ...
	robot.UnInit();

	// Uninitialize dog sound ...
	dog.UnInit();
}